/* gm_control.c -- Faz a interface entre servidor e robôs
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogo.ramos@usp.br>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 *
 * This file is part os GEARMedula.
 * 
 * GEARMedula is free software: you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation, version 3 of the
 * License.
 * 
 * GEARMedula is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see
 * <http://www.gnu.org/licenses/>.
 */

#include "gm_utils.h"
#include "gm_control.h"

#define NUMBER_OF_ROBOTS 5

/** Endereço dos robôs para o hardware  */
#define GM_CONTROL_END_ROBOT0 "BAL"
#define GM_CONTROL_END_ROBOT1 "BA1"
#define GM_CONTROL_END_ROBOT2 "BA2"
#define GM_CONTROL_END_ROBOT3 "BA3"
#define GM_CONTROL_END_ROBOT4 "BA4"

gm_control_t *
gm_control_new(const int f_real,
	       const char * serial_path)
{
    gm_control_t * self = NULL;
    int i = 0;

    self = XMALLOC( gm_control_t, 1 );

    /* Iniciando ponteiros */
    self->box = NULL;
    self-> f_real = 0;

    /* O servidor é real? */
    self->f_real = f_real;
       
    /* Iniciando os vetores que irão conter as informações para os
     * robôs */
    self->box_size = NUMBER_OF_ROBOTS;
    self->box = XMALLOC( gm_control_robot_t *, self->box_size );
    for( i = 0 ; i < NUMBER_OF_ROBOTS ; i++ )
	self->box[i] = NULL;

    /* Se o Servidor for real */
    if( self->f_real )
    {
	/* Criando robô 0 */
	self->box[0] = gm_control_robot_new( GM_CONTROL_END_ROBOT0, serial_path );

	/* Criando robô 1 */
	self->box[1] = gm_control_robot_new( GM_CONTROL_END_ROBOT1, serial_path );

	/* Criando robô 2 */
	self->box[2] = gm_control_robot_new( GM_CONTROL_END_ROBOT2, serial_path );

	/* Criando robô 3 */
	self->box[3] = gm_control_robot_new( GM_CONTROL_END_ROBOT3, serial_path );

	/* Criando robô 4 */
	self->box[4] = gm_control_robot_new( GM_CONTROL_END_ROBOT4, serial_path );
    }
    
    return self;
}

void
gm_control_destroy( gm_control_t * self )
{
    int i = 0;

    if( self->f_real )
    {
	for( i = 0 ; i < NUMBER_OF_ROBOTS ; i++ )
	{
	    gm_control_robot_destroy( self->box[i] );
	}
    }
    
    XFREE( self->box );
    XFREE( self );
}

gm_control_robot_t *
gm_control_robot_new( const char * end,
		      const char * serial_path )
{
    gm_control_robot_t * self = NULL;

    self = XMALLOC( gm_control_robot_t, 1 );

    self->serial = gm_serial_new( end, serial_path );

    return self;
}

void
gm_control_robot_destroy( gm_control_robot_t * self )
{
    gm_serial_destroy( self->serial );

    XFREE(self);
}

void
gm_control_robot_set_vel( gm_control_t * self,
			  const int id_robot,
			  const float vx,
			  const float vy,
			  const float va)
{
    if( self->f_real )
    {
	gm_serial_robot_set_vel( self->box[id_robot]->serial,
				 vx,
				 vy,
				 va );
    }
    else
    {
	gm_sim_robot_set_vel( id_robot,
			      vx,
			      vy,
			      va );
    }
}

void
gm_control_robot_kick( gm_control_t * self,
		       const int id_robot,
		       const int nivel)
{
    if( self->f_real )
    {
	gm_serial_robot_kick( self->box[id_robot]->serial,
			      nivel );
    }
    else
    {
	gm_sim_robot_kick( id_robot,
			   nivel );
    }
}

void
gm_control_robot_dribble_enable( gm_control_t * self,
				 const int id_robot,
				 const int enable)
{
    if( self->f_real )
    {
	gm_serial_robot_dribble_enable( self->box[id_robot]->serial,
					enable );
    }
    else
    {	
	gm_sim_robot_dribble_enable( id_robot,
				     enable );
    }
}

